/*
 *
 * Copyright (C) 2008 Ed Hong
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include "libQccPack.h"
#include "SubbandBits.h"
#include "math.h"

int bit_value(const SubbandBits *sb, int r, int c, int bp) {
  if (bp==-1) 
    return sb->coeff[r][c].sign == 1;
  else {
    int result = ((sb->coeff[r][c].bits & (1<<(31-bp))) != 0);
    return result;
  }
}

int bit_value_causal(const SubbandBits *sb, int r, int c, int bp) {
  int actualbp = bp;
  if (bp==-1) {
    actualbp = sb->coeff[r][c].significance_level;
  }
  if (sb->coeff[r][c].currbitplane > actualbp){
    return bit_value(sb,r,c,bp);
  }
  else return -1;
}


// modifies sb to store newbit. 
// returns 1 if newbit just caused the current coeff to become significant,
//  0 otherwise. 
int set_bit_value(int newbit, SubbandComponent *sc, int subband, int r, int c, int bp) {
  SubbandBits *sb = &(sc->sub[subband]);
  if (bp==-1) {
    sb->coeff[r][c].sign = 2*newbit-1;
    return 0;
  }
  else if (newbit==1) {
    int returnval =0; 

    if (sb->coeff[r][c].bits ==0) {
      returnval = 1;
    // also need to set significance level
      sb->coeff[r][c].significance_level = bp;
      // update descendant_significance by going through parents. 
      update_descendant_siglevels(sc, subband, r, c);
    }
    sb->coeff[r][c].bits |= (1<<(31-bp));
    return returnval;
  }
  else // 0 bit doesn't need to be stored. 
    return 0; 
  return 0;
}


int coeff_exists(const SubbandBits *sb, int r, int c)
{
  return (r  >= 0 ) && (c >= 0) && (r < sb->num_rows) && (c < sb->num_cols);
}

void computeChild(int s, int r, int c, int *childsub, int *childr, int *childc) {
  if (s == 0){ // LL subband
    *childsub = (r%2)+2*(c%2);
    if (childsub==0)  {
      //no children.. for this coeff
      *childsub = -1;
    }
    else {
      *childr = r- r%2;
      *childc = c - c%2;
    }	 
  } else {
    *childsub = s + 3;
    *childr = r*2;
    *childc = c*2;
  }
}

void computeParent(int s, int r, int c, int *psub, int *pr, int *pc)  {
    if (s >3) {
      *psub = s-3;
      *pr = r/2;
      *pc = c/2;
    }
    else if (s>0) {
      *psub = 0;
      *pr = r - r%2;
      *pc = c - c%2;
      
      if (s==3) {
	(*pr)++;
	(*pc)++;
      }
      else if (s == 2) {
	(*pc)++;
      }
      else {
	(*pr)++;
      }

    }
    else {// s <= 0
      *psub = 0;
      *pr = -1;
      *pc = -1;
    }
}



int num_children(const SubbandComponent*sc, int r, int c, int s) {
  int childsub, childrow, childcol;
  computeChild(s,r,c,&childsub, &childrow, &childcol);
  if (childsub >= sc->numSubbands) return 0;
  else 
      return coeff_exists(&(sc->sub[childsub]), childrow, childcol) +
      coeff_exists(&(sc->sub[childsub]), childrow+1, childcol) +
      coeff_exists(&(sc->sub[childsub]), childrow, childcol+1) +
      coeff_exists(&(sc->sub[childsub]), childrow+1, childcol+1);

}


// update descendant_significance of one newly sig coeff by going through parents. 
void update_descendant_siglevels (SubbandComponent *sc, 
					 int subband, int r, int c) {
      int parentsub, pi, pj;
      int i, j;
      i = r;
      j = c; 
      int done = 0;
      while (!done) {
	done = 1;
	computeParent(subband,i,j, &parentsub, &pi, &pj);
	if (coeff_exists(&(sc->sub[parentsub]), pi, pj)) {
	  if (sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel > 
	      sc->sub[subband].coeff[i][j].descendant_minsiglevel) {
	    sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel = 
	      sc->sub[subband].coeff[i][j].descendant_minsiglevel;
	    done = 0; // updated parent, check grandparent. 
	  } 
	  if (sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel > 
	      sc->sub[subband].coeff[i][j].significance_level) {
	    sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel = 
	      sc->sub[subband].coeff[i][j].significance_level;
	    done = 0; // updated parent, check grandparent. 
	  }
	  if (done == 0) {
	    subband = parentsub;
	    i = pi;
	    j = pj;
	  }
	}
      }
}


static void compute_descendant_siglevels (SubbandComponent *sc) {
  int s;
  int i,j;
  //initialize (not needed; now initialized in SubbandBitsAlloc.)
  for (s =0; s< sc->numSubbands;s++) {
    for (i=0; i < sc->sub[s].num_rows; i++) 
      for (j=0; j < sc->sub[s].num_cols; j++)
	sc->sub[s].coeff[i][j].descendant_minsiglevel = 32;
  }
  
  // bottom up traversal. This method is no longer used; 
  // although precomputing is faster way to do this for encoder, 
  //  encoder will now simulate decoder and compute on the fly. 
  //  so that contexts match up when using sorting. 
  for (s= sc->numSubbands-1; s>0; s--) {
    int parentsub;
    int pi, pj;
    for (i=0; i < sc->sub[s].num_rows; i++) 
      for (j=0; j < sc->sub[s].num_cols; j++) {
	computeParent(s,i,j, &parentsub, &pi, &pj);
	if (coeff_exists(&(sc->sub[parentsub]), pi, pj)) {
	    if (sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel > 
		sc->sub[s].coeff[i][j].descendant_minsiglevel) {
	      sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel = 
		sc->sub[s].coeff[i][j].descendant_minsiglevel;
	    } 
	    if (sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel > 
		sc->sub[s].coeff[i][j].significance_level) {
	      sc->sub[parentsub].coeff[pi][pj].descendant_minsiglevel = 
		sc->sub[s].coeff[i][j].significance_level;
	    } 
	}
      }
  }

}

int SubbandComponentInitialize(SubbandComponent *output, const QccWAVSubbandPyramid *subband_pyramid) {
  int NumSubbands;
  NumSubbands =     QccWAVSubbandPyramidNumLevelsToNumSubbands(subband_pyramid->num_levels);
  if (output == NULL) return 1;
  if (((output->sub) = (SubbandBits *)
       malloc(sizeof(SubbandBits)*NumSubbands)) == NULL)
    {
      QccErrorAddMessage("SubbandComponentInitialize: Error allocating memory");
      return(1);
    }
  
  output->numSubbands = NumSubbands;
  output->numCoeffs = subband_pyramid->num_rows * subband_pyramid->num_cols;
  int subband;

  for (subband = 0; subband < NumSubbands; subband++) {
    SubbandBitsInitialize(&output->sub[subband]);
    if (QccWAVSubbandPyramidSubbandSize(subband_pyramid,
                                          subband,
					&(output->sub[subband].num_rows),
					&(output->sub[subband].num_cols)))

        {
          QccErrorAddMessage("(SubbandComponentInitialize): Error calling QccWAVSubbandPyramidSubbandSize()");
          return(1);
        }
    //    printf("allocating subband %d, size %d x %d\n",subband, 
    //	   (output->sub[subband].num_rows),
    //	   (output->sub[subband].num_cols));
 
    SubbandBitsAlloc(&(output->sub[subband]));
  }
  return 0;
}

void  SubbandComponentFree(SubbandComponent *output) {
  int subband;
  for ( subband = 0; subband < output->numSubbands; subband++) {
      SubbandBitsFree(&output->sub[subband]);
  }
  QccFree(output->sub);
  output->sub = NULL;
  return;
}

// creates array of subbandbits and returns it. 
// assumes output is already created. 
int CreateBits(const QccWAVSubbandPyramid *sbp,
	       double firstthresh,	       
	       SubbandComponent *output, int debug) {
  QccIMGImageComponent    *output_images;

  int NumLevels = sbp->num_levels;
  int NumSubbands = QccWAVSubbandPyramidNumLevelsToNumSubbands(NumLevels);

  if (output == NULL) return 0;
      
  if (((output->sub) = (SubbandBits *)
       malloc(sizeof(SubbandBits)*NumSubbands)) == NULL)
    {
      QccErrorAddMessage("CreateBits: Error allocating memory");
      return(1);
    }
   
  output->numSubbands = NumSubbands;
  output->numCoeffs = sbp->num_rows * sbp->num_cols;

  if ((output_images = (QccIMGImageComponent *)
       malloc(sizeof(QccIMGImageComponent)*NumSubbands)) == NULL)
    {
      QccErrorAddMessage("CreateBits: Error allocating memory");
     return(1);
    }


  int subband;

  for (subband = 0; subband < NumSubbands; subband++) {
    QccIMGImageComponentInitialize(&output_images[subband]);
    SubbandBitsInitialize(&output->sub[subband]);
  }
  if (QccWAVSubbandPyramidSplitToImageComponent(sbp,
                                                output_images))
    {
      QccErrorAddMessage("CreateBits: Error calling QccWAVSubbandPyramidSplitToImageComponent()"
                         );
      return(1);
    }
    

  if (debug) printf("Initial Threshold: %f\n", firstthresh);

  for ( subband = 0; subband < NumSubbands; subband++)
    {
      if (debug ==1) printf ("Subband %d:\n", subband);
      if (ImageComponentConvertToBits(&output_images[subband], &output->sub[subband],firstthresh, debug))
        {
          QccErrorAddMessage("CreateBits: Error calling ImageComponentConvertToBits()");
          return(1);
        }
    }

  // do not pre-compute descendant_siglevels or insignificance levels;
  //  they will be updated on the fly so contexts computed will be in lock step with
  //  decoder.  note that desc_siglevels and insig levels are initialized
  //    already through SubbandBitsAlloc called by convertToBits. 

  if (debug ==2) printSubbandComponent(output);


  for ( subband = 0; subband < NumSubbands; subband++)
    QccIMGImageComponentFree(&output_images[subband]);

  QccFree(output_images);
  return 0;
}

void printSubbandComponent(const SubbandComponent *output) {
  int i,j;
  int subband;
  for ( subband = 0; subband < output->numSubbands; subband++) {
    SubbandBits * sb;
    sb = &(output->sub[subband]);
    for ( i=0; i < sb->num_rows; i++) {
      for ( j =0; j < sb->num_cols; j++) {
	printf(" bits: %8X, sign: %2d, siglevel:%2d, descminsigl:%2d, %d:(%d,%d)\n", 
	       sb->coeff[i][j].bits,
	       sb->coeff[i][j].sign,
	       sb->coeff[i][j].significance_level,
	       sb->coeff[i][j].descendant_minsiglevel,
	       subband,i,j);
	  
      }
    }
  }
}

void SubbandBitsInitialize(SubbandBits *sbits) {
  if (sbits == NULL) return;
  sbits->num_rows = 0;
  sbits->num_cols = 0;
  sbits->coeff = NULL;
}

void SubbandBitsFree(SubbandBits *sbits) {
  if (sbits == NULL) return;
  if (sbits->coeff != NULL) {
    int t = 0;
    for (t=0; t < sbits->num_rows; t++) {
      QccFree(sbits->coeff[t]);
    }
    QccFree(sbits->coeff);
  }
 
  return;

}

int SubbandBitsAlloc(SubbandBits *sb) {
  if ((sb->coeff = (CoefficientData **) malloc(sizeof(CoefficientData *)*sb->num_rows )) == NULL)
    {
      QccErrorAddMessage("SubbandBitsAlloc: Error allocating memory");
      return 1;
    }
  
  int t;
  for (t=0; t < sb->num_rows; t++) {
    if ((sb->coeff[t] = (CoefficientData *) malloc(sizeof(CoefficientData )*sb->num_cols )) == NULL)
      {
	QccErrorAddMessage("SubbandBitsAlloc: Error allocating memory");
	return 1;
      }
  }

  int i,j;
  CoefficientData prototype;
  prototype.sign = 0;
  prototype.bits = 0;
  prototype.significance_level =32;
  prototype.descendant_minsiglevel = 32;
  prototype.currbitplane = 0;
  prototype.locator = NULL;

  for (i = 0; i < MAXBITPLANES; i++) {
    prototype.insignificance_level[i] = 0;
  }

  //initialize. 
  for (i=0; i < sb->num_rows; i++) 
    for (j=0; j < sb->num_cols; j++) {	
      sb->coeff[i][j]= prototype;
    }
  
  return 0;
}


// assumes maxmag is positive. returns power x such that
//  2^x is initial threshold. 
int computeInitialThreshold(const QccWAVSubbandPyramid * sbp, int numComponents) {

  double coeffmax, coeffmin;
  coeffmax = QccMatrixMaxValue(sbp->matrix, sbp->num_rows, sbp->num_cols);
  coeffmin=QccMatrixMinValue(sbp->matrix, sbp->num_rows, sbp->num_cols);
  if (numComponents > 1) {
    int i;
    double temp;
    for (i=1; i < numComponents; i++) {
      temp = QccMatrixMaxValue(sbp[i].matrix, sbp[i].num_rows, sbp[i].num_cols);
      coeffmax = (temp > coeffmax)? temp : coeffmax;
      temp = QccMatrixMinValue(sbp[i].matrix, sbp[i].num_rows, sbp[i].num_cols);
      coeffmin = (temp < coeffmin)? temp : coeffmin;
    }
  }
  
  double coeffmaxmag;
  coeffmax = fabs(coeffmax);
  coeffmin = fabs(coeffmin);
  coeffmaxmag = (coeffmax > coeffmin) ? coeffmax : coeffmin;

  double maxmag = coeffmaxmag;

  //      printf("Maxmag is %f\n", maxmag);
  int log2mag = (int) (log(maxmag)/M_LN2);
  //  printf("log2mag is %d\n", log2mag);
  double result = pow(2,log2mag);
  //  printf("result is %f\n", result);
  if (result == maxmag)
    log2mag--;// for when maxmag is an exact power of 2. 
  return log2mag;
}

// fills in one SubbandBits data structure according to imgcmp
// firstthreshold represents the threshold level in magnitude for the first bitplane

int ImageComponentConvertToBits(const QccIMGImageComponent *imgcmp, SubbandBits *sb, double firstthreshold, int debug) {

  sb->num_rows = imgcmp->num_rows;
  sb->num_cols = imgcmp->num_cols;
  if (SubbandBitsAlloc(sb)) return 1;
  
  double maxuint = pow(2,32);
  
  int i,j;
  for ( i=0; i < sb->num_rows; i++) {
    for ( j =0; j < sb->num_cols; j++) {
      double currcoeff = imgcmp->image[i][j];
      if (currcoeff <0) 
	sb->coeff[i][j].sign = -1;
      else 
	sb->coeff[i][j].sign = 1;
      
      double normalizedcoeff = fabs(currcoeff / (2.0*firstthreshold));
      if (normalizedcoeff >= 1.0 - 1.0/maxuint) {
	printf("ImageComponentConvertToBits: Warning, firstthreshold is too small for currcoeff\n");
	sb->coeff[i][j].bits = 0xFFFFFFFF;
      }
      else if (normalizedcoeff < 0.0) {
	QccErrorAddMessage("ImageComponentConvertToBits: Error, normalized coeff < 0");
	sb->coeff[i][j].bits = 0;
      } 
      else {
	sb->coeff[i][j].bits = (unsigned int)floor(maxuint * normalizedcoeff+0.5);
      }
      sb->coeff[i][j].significance_level = 0;
      int currbit = 31;
      while ( (sb->coeff[i][j].bits & (1 << currbit))==0) {
	sb->coeff[i][j].significance_level++;
	currbit--;
	if (currbit == -1) { // no significant bits 
	  sb->coeff[i][j].significance_level = 32;
	  sb->coeff[i][j].sign = 0;
	  break;
	}
      }
      if (debug==1) printf("orig: %11f, bits: %8X, sign: %d, siglevel:%d, (%d,%d)\n", 
			imgcmp->image[i][j],
			sb->coeff[i][j].bits,
			sb->coeff[i][j].sign,
			sb->coeff[i][j].significance_level,i,j);
    }
  }
  return 0;
}

void printsbploc(const sbploc*l) {
  printf("bp=%d, %d:(%d,%d) %d\n", l->k, l->subband, l->i, l->j, l->comp);
}

void copySubbandComponentToSubbandPyramid(QccWAVSubbandPyramid *sbp, 
					  const SubbandComponent *sc, const sbploc *loc, 
					  int initialthreshpower) {
  /// quantizaton bias at various bitplanes; as used by amir said in SPIHT. 
  double qbias[] = {0.3863, 0.4393, 0.4690, 0.4844, 0.4922, 0.4961};
  int debug = 0;
  //  printsbploc(loc);
  double initialthresh = 1<<initialthreshpower; 
  int s, i, j; 
  int bp;
  for (s = 0; s < sc->numSubbands; s++) {
    int rowoff, coloff;
    if (
	QccWAVSubbandPyramidSubbandOffsets(sbp, s, &rowoff, &coloff)) {
      printf("copysubbandComponentToSubbandPyramid: call to QccWavSubbandPyramidSubbandOffsets failed \n");
      exit(1);
    }    
      for (i=0; i < sc->sub[s].num_rows; i++) {
	for (j=0; j < sc->sub[s].num_cols; j++) {
	  // bp represent the bitplane of the last bit coded for the current coeff.
	  bp = sc->sub[s].coeff[i][j].currbitplane -1;
	  double value; 
	  value = sc->sub[s].coeff[i][j].bits >> (31-bp);
	  if (value > 0) {
	    // sig coeffs get rounded to middle of interval. 
	    //	    value += .5; 
            // sig coeffs now get bias adjusted according to 
            // qbias, or formula 0.5 - (0.25)/(k+1)^2, where k= bitplane number (starting at 0). 
            if (bp < 6) {
                value += qbias[bp];
            }
            else if (value ==1) {
               value += .42;  // heuristic adjustment. 
            }
            else {
                value += (0.5 - (0.25)/(bp+1)/(bp+1));
            }
            /* obsolete heuristic reconstruction adjustment
	    if (value ==1) {
	      value += .42;
	    } 
	    else 
	      value +=.5; */
	    value *= (initialthresh)/(1<<bp);
	    value = value * sc->sub[s].coeff[i][j].sign;
	    if (debug) 
	      printf("%12f, %8X %2d %2d %2d\n", value, sc->sub[s].coeff[i][j].bits, s, i, j); 
	  }
	  sbp->matrix[i+rowoff][j+coloff] = value;
	}
    }
  }

}

int computeNumWaveletLevels(int rows, int cols) {
  int length = (rows<cols)? rows : cols;
  int result =0;
  while (length >8) {
    length /=2;
    result++;
  }
  return result;
}
